#include <libplayerc++/playerc++.h>
#include <libplayerinterface/player.h>
#include <boost/thread/thread.hpp>

#include <core/synced_current_pose.h>
#include <core/test/planner_reactive_localization_test.h>
#include <localization/localization.h>
#include <localization/maps/lamps_maps.h>
#include <path_planning/path_planner.h>
#include <reactive_behavior/reactive_behavior.h>
#include <vision/vision_system.h>

using std::cout;
using std::cin;
using std::endl;

using PlayerCc::dtor;

// TODO REMOVE ME
int main(int argc, char* argv[]) {
  TESTPlannerReactiveLocalization(argc, argv);
  return 0;
}

void TESTPlannerReactiveLocalization(int argc, char* argv[]) {
  player_point_2d initial_position, goal_position;
  double initial_yaw, goal_yaw;

  // ----------------------------- Player Setup ------------------------------
  PlayerCc::PlayerClient player_client("localhost");
  PlayerCc::Position2dProxy position2d_proxy(&player_client);
  PlayerCc::SonarProxy sonar_proxy(&player_client);

  position2d_proxy.RequestGeom();
  position2d_proxy.ResetOdometry();
  sonar_proxy.RequestGeom();
  player_client.Read();
  player_client.Read();
  position2d_proxy.SetMotorEnable(1);

  // ------------------------ Localization and Vision ------------------------
  cout << "Initial pose (px py pa), 'pa' in degrees: ";
  cin >> initial_position.px >> initial_position.py >> initial_yaw;
  // initial_position.px = 0.0; initial_position.py = 0.0; initial_yaw = 90.0;
  initial_yaw = dtor(initial_yaw);
  cout << "Goal pose (px py pa), 'pa', in degrees: ";
  cin >> goal_position.px >> goal_position.py >> goal_yaw;
  // goal_position.px = 0.0; goal_position.py = 16.0; goal_yaw = 90.0;
  goal_yaw = dtor(goal_yaw);

  sleep(3);

  gsl_matrix *initial_covariance = gsl_matrix_alloc(3, 3);
  gsl_matrix_set_zero(initial_covariance);
  gsl_matrix_set(initial_covariance, 0, 0, 0.01);
  gsl_matrix_set(initial_covariance, 1, 1, 0.01);
  gsl_matrix_set(initial_covariance, 2, 2, 0.0001);

  SyncedCurrentPose synced_current_pose;
  player_pose2d initial_pose = {initial_position.px, initial_position.py, initial_yaw};
  synced_current_pose.set_current_pose(initial_pose);
  SyncedCurrentCovariance synced_current_covariance;
  synced_current_covariance.set_current_covariance(initial_covariance);
  SyncedLampCentroids synced_lamp_centroids;

  LocalizationEstimator localization_estimator(&initial_pose,
            initial_covariance, MappedLampsBBlock(), &player_client, &position2d_proxy,
            &synced_current_pose, &synced_current_covariance, &synced_lamp_centroids);
  VisionSystem vision_system(&synced_lamp_centroids);

  boost::thread localization_estimator_thread(boost::ref(localization_estimator));
  boost::thread vision_system_thread(boost::ref(vision_system));

  // ------------------------- Reactive and Planner --------------------------
  ReactiveBehavior reactive_behavior(&player_client, &position2d_proxy,
          &sonar_proxy, &synced_current_pose);
  PathPlanner path_planner(argv[1], argv[2]);

  // Request a plan and follow it using reactive behavior.
  WaypointList *waypoint_list = path_planner.RequestWaypoints(initial_position, goal_position);
  player_point_2d *waypoint;
  while ((waypoint = waypoint_list->Next()) != NULL)
    reactive_behavior.MoveToGoalAvoidingObstacles(*waypoint);

  // Move from last waypoint to goal.
  //reactive_behavior.MoveToGoalAvoidingObstacles(goal_position);
  //reactive_behavior.RotateTo(goal_yaw);
}
